#!/usr/bin/env python
import roslib; roslib.load_manifest('sushi_tutorials')
import rospy
from pr2_tasks.tasks import Tasks
from pr2_tasks.pickplace import PickPlace
from pr2_tasks.tableware_detection import TablewareDetection
from pr2_tasks.pickplace_definitions import PlaceGoal
from geometry_msgs.msg import PointStamped
#from arm_navigation_msgs import OrderedCollisionOperations, CollisionOperation
import copy

def test1():
    #find objects on the table
    head_point = PointStamped()
    head_point.header.frame_id = '/torso_lift_link'
    head_point.point.x = 0.8
    detector = TablewareDetection()
    det = detector.detect_objects(point_head_at=head_point)
    if not det.pickup_goals:
        rospy.loginfo('Nothing to pick up!')
        return

    #pick up the first object that we found
    pickplace = PickPlace()
    pg = det.pickup_goals[0]
    pg.arm_name = 'right_arm'
    pickplace.pickup(pg)


    #pick up the first object that we found
    pg = det.pickup_goals[0]
    pg.arm_name = 'left_arm'
    pickplace.pickup(pg)


def test2():
    #find objects on the table
    head_point = PointStamped()
    head_point.header.frame_id = '/torso_lift_link'
    head_point.point.x = 0.8
    detector = TablewareDetection()
    det = detector.detect_objects(point_head_at=head_point)
    if not det.pickup_goals:
        rospy.loginfo('Nothing to pick up!')
        return

    col1 = CollisionOperation(object1 = "right_arm_gripper_r_finger_tip_link", object2="graspable", operation = CollisionOperation.ENABLE) 
    col2 = CollisionOperation(object1 = "right_arm_gripper_l_finger_tip_link", object2="graspable", operation = CollisionOperation.ENABLE) 
    col3 = CollisionOperation(object1 = "left_arm_gripper_r_finger_tip_link", object2="graspable", operation = CollisionOperation.ENABLE) 
    col4 = CollisionOperation(object1 = "left_arm_gripper_l_finger_tip_link", object2="graspable", operation = CollisionOperation.ENABLE) 
    collisions = OrderedCollisionOperations([col1, col2, col3, col4])
    #pick up the first object that we found
    pickplace = PickPlace()
    pg = det.pickup_goals[0]
    pg.arm_name = 'right_arm'
    pg.min_lift_distance = 0.005
    pg.desired_lift_distance = 0.01
    pg.additional_collision_operations = collisions
    pickplace.pickup(pg)


    #pick up the first object that we found
    pg = det.pickup_goals[0]
    pg.arm_name = 'left_arm'
    pg.min_lift_distance = 0.005
    pg.desired_lift_distance = 0.01
    pg.additional_collision_operations = collisions
    pickplace.pickup(pg)

def test3():
    #find objects on the table
    head_point = PointStamped()
    head_point.header.frame_id = '/torso_lift_link'
    head_point.point.x = 0.8
    detector = TablewareDetection()
    det = detector.detect_objects(point_head_at=head_point)
    if not det.pickup_goals:
        rospy.loginfo('Nothing to pick up!')
        return

    #pick up the first object that we found
    pickplace = PickPlace()
    pg = det.pickup_goals[0]
    pg.arm_name = 'right_arm'
    pg.min_lift_distance = 0.005
    pg.ignore_collisions = True
    pg.desired_lift_distance = 0.01
    pickplace.pickup(pg)


    #pick up the first object that we found
    pg = det.pickup_goals[0]
    pg.arm_name = 'left_arm'
    pg.min_lift_distance = 0.005
    pg.desired_lift_distance = 0.01
    pg.ignore_collisions = True
    pickplace.pickup(pg)


def test4():
    #find objects on the table
    head_point = PointStamped()
    head_point.header.frame_id = '/torso_lift_link'
    head_point.point.x = 0.8
    detector = TablewareDetection()
    det = detector.detect_objects(point_head_at=head_point)
    if not det.pickup_goals:
        rospy.loginfo('Nothing to pick up!')
        return

    #pick up the first object that we found
    pickplace = PickPlace()
    pg = det.pickup_goals[0]
    print pg.desired_grasps
     
    pg.arm_name = 'right_arm'
    pg.lift.min_distance = 0.005
    pg.lift.desired_distance = 0.01
    pickplace.pickup(pg)


    #pick up the first object that we found
    pg = det.pickup_goals[0]
    pg.arm_name = 'left_arm'
    pg.lift.min_distance = 0.005
    pg.lift.desired_distance = 0.01
    pickplace.pickup(pg)


rospy.init_node('two_arm_pickplace_tester_node')

test_switch = 4
if test_switch == 1:
   rospy.loginfo("Attempting to pickup tray without additional contraints")
   test1()
elif test_switch == 2:
   rospy.loginfo("Attempting to pickup tray with additional contraints")
   rospy.loginfo("set min_lift_distance, desired_lift_distance, and additional_collision_operations")
   test2()
elif test_switch == 3:
   rospy.loginfo("Attempting to pickup tray with additional contraints")
   rospy.loginfo("set min_lift_distance, desired_lift_distance, and ignore_collisions")
   test3()
elif test_switch == 4:
   rospy.loginfo("Attempting to pickup tray with additional contraints")
   rospy.loginfo("set min_lift_distance, desired_lift_distance")
   test4()





